namespace Test.Robotics.Navigation
{
    using System;
    using System.Runtime.Serialization;
    using System.Threading.Tasks;

    using Microsoft.Robotics.Hardware.DAC;
    using Microsoft.Robotics.Hardware.Runtime.DAC;
    using Microsoft.Robotics.Interaction.Runtime.Input;
    using Microsoft.Robotics.Navigation.Motion;
    using Microsoft.Robotics.Navigation.Runtime.Motion;
    using Microsoft.Robotics.Numerics;
    using Microsoft.Robotics.Runtime;
    using Microsoft.Robotics.Vision.Runtime.Cameras;

    /// <summary>
    /// Implements simple gamepad controller - based manual joystick drive agent intended to be fed into LocalSuggestionDrive (or just drive robot around)
    /// Left thumb stick controls forward speed - right thumb stick - rotation
    /// </summary>
    [DataContract]
    public class GamepadDriveAgent : ConsumerProducerAgent<GamePadAgentMessage, MobileBaseVelocityReferenceMessage>
    {
        /// <summary>
        /// Max drive max speed in meters per second
        /// </summary>
        [DataMember(Name = "DriveSpeedMax")]
        private double? driveSpeedMax;

        /// <summary>
        /// Max rotation max speed in radians per second
        /// </summary>
        [DataMember(Name = "DriveRotationMax")]
        private double? driveRotationMax;

        /// <summary>
        /// Initializes a new instance of the <see cref="GamepadDriveAgent"/> class.
        /// </summary>
        /// <param name="name">The agent name</param>
        /// <param name="producer">The producer name</param>
        public GamepadDriveAgent(string name, string producer)
            : base(name, producer)
        {
        }

        /// <summary>
        /// Initializes a new instance of the <see cref="GamepadDriveAgent"/> class.
        /// </summary>
        /// <param name="name">Agent name</param>
        /// <param name="gamePadProducer">The game pad producer</param>
        /// <param name="driveSpeedMax">Drive speed max</param>
        /// <param name="driveRotationMax">Drive rotation max</param>        
        public GamepadDriveAgent(
            string name,
            string gamePadProducer,
            double driveSpeedMax,
            double driveRotationMax)
            : base(name, gamePadProducer)
        {
            this.driveSpeedMax = driveSpeedMax;
            this.driveRotationMax = driveRotationMax;
        }

        /// <summary>
        /// Called on agent initialization
        /// </summary>
        /// <param name="locator">The agent locator.</param>
        public override void Initialize(AgentLocator locator)
        {
            if (!this.driveSpeedMax.HasValue ||
                !this.driveRotationMax.HasValue)
            {
                throw new ArgumentException(string.Format("At least one required initialization parameter is missing for {0}.", this.GetType().FullName));
            }

            base.Initialize(locator);
        }

        /// <summary>
        /// Called when the agent is activated
        /// </summary>
        public override void OnActivated()
        {
            base.OnActivated();
        }

        /// <summary>
        /// Handle game pad messages
        /// </summary>
        /// <param name="message">Message with game pad state</param>
        public override void Receive(GamePadAgentMessage message)
        {
            this.ControlRobotMotion(message);
        }

        /// <summary>
        /// Drive robot with thumb sticks
        /// </summary>
        /// <param name="message">Gamepad Message</param>
        private void ControlRobotMotion(GamePadAgentMessage message)
        {
            float xThumb = message.ThumbSticks.Right.X;
            float yThumb = message.ThumbSticks.Left.Y;            

            double forwardVelocity = this.driveSpeedMax.Value * yThumb;
            double rotVelocity = -this.driveRotationMax.Value * xThumb;

            MobileBaseVelocityReference velocityUpdate = new MobileBaseVelocityReference(forwardVelocity, 0, rotVelocity);

            MobileBaseVelocityReferenceMessage newMessage = new MobileBaseVelocityReferenceMessage(velocityUpdate, message);

            this.Publisher.Post(newMessage);
        }        
    }
}
